#!/bin/bash

# 检查参数数量最少为2个
if [ $# -lt 2 ]; then
    echo "用法: $0 <模块名> <操作编号>"
    echo "模块名可选: sensor 或 RMS"
    echo "操作编号说明:"
    echo "  1: 构建该模块"
    echo "  2: 启动发布者节点"
    echo "  3: 启动订阅者/监听者节点"
    exit 1
fi

MODULE=$1
ACTION=$2

# 处理 RMS 模块
if [ "$MODULE" == "rms" ]; then
    case $ACTION in
        1)
            echo "构建 rms_pkg 包..."
            colcon build --packages-select rms_pkg
            ;;
        2)
            echo "启动 rms_pkg 节点..."
	    source install/setup.bash
            ros2 run rms_pkg rms_pkg --ros-args \
                -p robot_id:="robot1001" \
                -p robot_type:="inspection" \
                -p interval:=2 \
                -p x:=-10.0 \
                -p y:=-20.0 \
                --log-level DEBUG
            ;;
        3)
            echo "启动 rms_pkg 节点..."
	    source install/setup.bash
            ros2 run rms_pkg rms_pkg --ros-args \
                -p timeout:=5 \
                --log-level DEBUG
            ;;
        4)
            echo "启动 sensor_m01h20_publisher 节点..."
	    source install/setup.bash
            ros2 run rms_pkg sensor_m01h20_publisher --ros-args \
                -p serial_port:=/dev/ttyCH341USB0 \
                -p baud_rate:=9600 \
                -p device_address:=0x1 \
                -p read_interval:=5 \
                --log-level DEBUG
            ;;
        5)
            echo "启动 multi_node_launch.py (包含心跳、环境传感器节点、状态Service节点、任务Action Service节点、logs server节点)..."
	    source install/setup.bash
            # 启动 robot1001
            #ros2 launch rms_pkg multi_node_launch.py robot_id:=robot1002
            ros2 launch rms_pkg multi_node_launch.py
            ;;
        6)
            echo "启动 livox_mid360_3d.launch.py (使用cartographer_ros和Livox mid-360建图)..."
	    source install/setup.bash
            # 使用cartographer_ros和Livox mid-360建图
            ros2 launch rms_pkg livox_mid360_3d.launch.py
            ;;
        *)
            echo "错误: RMS模块不支持的操作编号: $ACTION"
            exit 1
            ;;
    esac

# 处理 sensor 模块
elif [ "$MODULE" == "sensor" ]; then
    case $ACTION in
        1)
            echo "构建 sensor_cpp_pkg 包..."
            colcon build --packages-select sensor_cpp_pkg
            ;;
        2)
            echo "启动 sensor_hcd5014_publisher 节点..."
	    source install/setup.bash
            ros2 run rms_pkg sensor_hcd5014_publisher --ros-args \
                -p serial_port:=/dev/ttyUSB0 \
                -p baud_rate:=9600 \
                -p device_address:=0x33 \
                -p read_interval:=5 \
                --log-level DEBUG
            ;;
        3)
            echo "启动 sensor_hcd5014_subscriber 节点..."
	    source install/setup.bash
            ros2 run rms_pkg sensor_hcd5014_subscriber --ros-args \
                --log-level DEBUG
            ;;
        *)
            echo "错误: sensor模块不支持的操作编号: $ACTION"
            exit 1
            ;;
    esac

    # 处理 service 模块
elif [ "$MODULE" == "service" ]; then
    case $ACTION in
        1)
            echo "构建 robot_status_server 包..."
            colcon build --packages-select robot_status_server
            ;;
        2)
            echo "启动 robot_status_server 节点..."
	    source install/setup.bash
            ros2 run rms_pkg robot_status_server --ros-args \
                -p seer_ip:=192.168.48.101 \
                -p stat_port:=19204 \
                --log-level DEBUG
            ;;
        3)
            echo "启动 robot_logs_server 节点..."
	    source install/setup.bash
            ros2 run rms_pkg robot_logs_server --ros-args \
                --log-level DEBUG
            ;;
        *)
            echo "错误: robot_status_server模块不支持的操作编号: $ACTION"
            exit 1
            ;;
    esac

    # 处理 action 模块
elif [ "$MODULE" == "action" ]; then
    case $ACTION in
        1)
            echo "构建 task_server_node 包..."
            colcon build --packages-select task_action_server
            ;;
        2)
            echo "启动 task_server_node 节点..."
	    source install/setup.bash
            ros2 run rms_pkg task_action_server --ros-args \
            	-r __ns:=/robot1001 \
                -p seer_ip:=192.168.48.101 \
                -p yolo_ip:=192.168.48.101 \
                -p navi_port:=19206 \
                -p conf_port:=19204 \
                -p spraying_gpio_id:=3 \
                --log-level DEBUG
            ;;
        3)
            echo "启动 task_server_node 节点..."
	    source install/setup.bash
            ros2 run rms_pkg task_action_server --ros-args \
                --log-level DEBUG
            ;;
        *)
            echo "错误: task_action_server模块不支持的操作编号: $ACTION"
            exit 1
            ;;
    esac

# 模块名错误
else
    echo "错误: 不支持的模块名 '$MODULE'"
    echo "可用模块: RMS sensor"
    exit 1
fi

# 检查执行结果
if [ $? -ne 0 ]; then
    echo "操作失败！"
    exit 1
else
    echo "操作完成。"
fi
